#include "LaserFinder.h"

#include <fstream> 
#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#define ESC 27
#define ENTER 10

// рисуем целеуказатель
void LaserFinder :: drawTarget(IplImage* img, const int x, const int y, const int radius) const{
    cvCircle(img, cvPoint(x, y), radius, CV_RGB(250, 0, 0), 1, 8);
    cvLine(img, cvPoint(x-radius/2, y-radius/2), cvPoint(x+radius/2, y+radius/2),CV_RGB(250,0,0),1,8);
    cvLine(img, cvPoint(x-radius/2, y+radius/2), cvPoint(x+radius/2, y-radius/2),CV_RGB(250,0,0),1,8);
}

// рисуем трек
void LaserFinder :: drawTrack(IplImage* img) const{
//    for (std::vector<CvPoint>::iterator it = track.begin(); it != track.end(); ++it){
//        cvCircle(img, *it, 1, CV_RGB(250, 0, 0), 1, 8);
//    }
    for (int it = 0; it < track.size() - 1; ++it){
        cvCircle(img, track[it], 3, CV_RGB(250, 0, 0), 1, 8);
        cvLine(img, track[it], track[it + 1], CV_RGB(250, 0, 0), 1, 8);
    }
    cvCircle(img, track[track.size() - 1], 3, CV_RGB(250, 0, 0), 1, 8);
    
}

// вывод текста
void LaserFinder :: printText(IplImage* img, const int x, const int y, const char* text) const{
    // задаём точку для вывода текста
    CvPoint pt = cvPoint( x, y );
    // инициализация шрифта
    CvFont font;
    cvInitFont( &font, CV_FONT_HERSHEY_COMPLEX,1.0, 1.0, 0, 1, CV_AA);
    // используя шрифт выводим на картинку текст
    cvPutText(img, text, pt, &font, CV_RGB(0, 250, 0) );
}

//строим изображение hsv_and для поиска пятна
void LaserFinder :: hsvFinder (const IplImage* image, IplImage* hsv_and) const{
    // для хранения каналов HSV
    IplImage* hsv = 0;
    IplImage* h_plane = 0;
    IplImage* s_plane = 0;
    IplImage* v_plane = 0;
    // для хранения каналов HSV после преобразования
    IplImage* h_range = 0;
    IplImage* s_range = 0;
    IplImage* v_range = 0;

    assert( image != 0 );

    // создаём картинки
    hsv = cvCreateImage( cvGetSize(image), IPL_DEPTH_8U, 3 );
    h_plane = cvCreateImage( cvGetSize(image), IPL_DEPTH_8U, 1 );
    s_plane = cvCreateImage( cvGetSize(image), IPL_DEPTH_8U, 1 );
    v_plane = cvCreateImage( cvGetSize(image), IPL_DEPTH_8U, 1 );
    h_range = cvCreateImage( cvGetSize(image), IPL_DEPTH_8U, 1 );
    s_range = cvCreateImage( cvGetSize(image), IPL_DEPTH_8U, 1 );
    v_range = cvCreateImage( cvGetSize(image), IPL_DEPTH_8U, 1 );
    //  конвертируем в HSV 
    cvCvtColor( image, hsv, CV_BGR2HSV ); 
    // разбиваем на отельные каналы
    cvCvtPixToPlane( hsv, h_plane, s_plane, v_plane, 0 );

    cvInRangeS(h_plane, cvScalar(Hmin), cvScalar(Hmax), h_range);
    cvInRangeS(s_plane, cvScalar(Smin), cvScalar(Smax), s_range);
    cvInRangeS(v_plane, cvScalar(Vmin), cvScalar(Vmax), v_range);

    // складываем 
    cvAnd(h_range, s_range, hsv_and);
    cvAnd(hsv_and, v_range, hsv_and);

    // освобождаем ресурсы
    cvReleaseImage(&hsv);
    cvReleaseImage(&h_plane);
    cvReleaseImage(&s_plane);
    cvReleaseImage(&v_plane);
    cvReleaseImage(&h_range);
    cvReleaseImage(&s_range);
    cvReleaseImage(&v_range);
}

//находим белое пятно
void LaserFinder :: brightFinder (const IplImage* img, int* x, int* y) const{
    int height    = img->height;
    int width     = img->width;
    int step      = img->widthStep;
    int channels  = img->nChannels;
    uchar *data   = (uchar *)img->imageData;

    //Ищем самое яркое пятно
    int min = 255;
    int minX = 0;
    int minY = 0;

    for(int i = 0; i < height; i++){ 
        for(int j = 0; j < width; j++){
            for(int k = 0; k < channels; k++){
                if(data[i*step + j*channels + k] >= min){
                    min = data[i*step + j*channels + k];
                    minX = j;
                    minY = i;
                } 
            }
        }
    } 
    *x = minX;
    *y = minY;
}

void LaserFinder :: setTransformMatrix (const IplImage* frame){
    // точки
    CvPoint2D32f frameQuad[4], dstQuad[4];

    // задаём точки 640x480
    frameQuad[0].x = topLeftX;           //frame Top left
    frameQuad[0].y = topLeftY;
    frameQuad[1].x = topRightX;           //frame Top right
    frameQuad[1].y = topRightY;
    frameQuad[2].x = bottomLeftX;           //frame Bottom left
    frameQuad[2].y = bottomLeftY;
    frameQuad[3].x = bottomRightX;           //frame Bot right
    frameQuad[3].y = bottomRightY;
    //- - - - - - - - - - - - - -//
    dstQuad[0].x = 0;               //dst Top left
    dstQuad[0].y = 0;
    dstQuad[1].x = resultSizeX;    //dst Top right
    dstQuad[1].y = 0;
    dstQuad[2].x = 0;               //dst Bottom left
    dstQuad[2].y = resultSizeY;      
    dstQuad[3].x = resultSizeX;    //dst Bot right
    dstQuad[3].y = resultSizeY;

    // получаем матрицу преобразования
    cvGetPerspectiveTransform(frameQuad, dstQuad, warp_matrix);
}

//трансформируем четырехугольник экрана в прямоугольник
void LaserFinder :: perspectiveTransform (const IplImage* frame, IplImage* dst) const{
    // преобразование перспективы
    cvWarpPerspective(frame, dst, warp_matrix);
}

//ищем самое близкое к х,у пятно с похожим значением цвета 
bool LaserFinder :: findAlike(const IplImage* img, int* newx, int* newy) const{
    int dx = 15; //рассматриваемая область по оси x
    int dy = 15; //рассматриваема область по оси y
    int dC1 = 1;//допустимое отклониение по первому каналу
    int dC2 = 1;//допустимое отклониение по второму каналу
    int dC3 = 1;//допустимое отклониение по третьему каналу
    int x = prevX;
    int y = prevY; 
    int c1 = prevH;
    int c2 = prevS;
    int c3 = prevV;

    IplImage* img_hsv = 0;
    img_hsv = cvCreateImage( cvGetSize(img), IPL_DEPTH_8U, 3 );

    cvCvtColor( img, img_hsv, CV_BGR2HSV ); 

    int height    = img_hsv->height;
    int width     = img_hsv->width;
    int step      = img_hsv->widthStep;
    int channels  = img_hsv->nChannels;
    uchar *data   = (uchar *)img_hsv->imageData;

    int tmpx = x;
    int tmpy = y;

    bool ifFound = false;

    int left = (x-dx > 0) ? (x-dx) : 0;
    int right = (x+dx < width) ? (x+dx) : width;
    int top = (y-dy > 0) ? (y-dy) : 0;
    int bottom = (y+dy < height) ? (y+dy) : height;

    //бежим по всем пикселям из рассматриваемой области
    for(int i = left; i < right; i++){
        for(int j = top; j < bottom; j++){
            int dataC1 = data[i*step + j*channels + 0];
            int dataC2 = data[i*step + j*channels + 1];
            int dataC3 = data[i*step + j*channels + 2];
            //если нашли пиксель похожего цвета
            if( (abs(dataC1-c1) < dC1) && (abs(dataC2-c2) < dC2) && (abs(dataC3-c3) < dC3)){
                if (ifFound == false){
                    tmpx = i;
                    tmpy = j;
                }else{
                    //если текущий найденный пиксель ближе к исходной точке, чем ранее найденный
                    if (((x-tmpx)*(x-tmpx)+(y-tmpy)*(y-tmpy)) > ((x-i)*(x-i)+(y-j)*(y-j))){
                        tmpx = i;
                        tmpy = j;
                    }
                }            
                ifFound = true;
            }
        }
    }
    *newx = tmpx;
    *newy = tmpy;
    // !!!
    cvReleaseImage(&img_hsv);
    return ifFound;
}

//обработчик лазерной указки для одного изображения, статически
bool LaserFinder :: findLaser (IplImage* dst, int* minX, int* minY) const{
    // Здесь 1 - количество каналов  
    IplImage* hsv_and = cvCreateImage( cvGetSize(dst), IPL_DEPTH_8U, 1 );  
    hsvFinder(dst, hsv_and);
    //cvCvtColor(dst, hsv_and, CV_RGB2GRAY);
    brightFinder(hsv_and, minX, minY);
    // Освобождаем ресурсы  
    cvReleaseImage(&hsv_and);  
    if ((*minX == 0) && (*minY == 0)){
        return false;
    }
    return true;
}

void LaserFinder :: setCorners(){
    IplImage* img  = 0;
    img = getImage(); 
    if (img) {
        setTransformMatrix (img);
    }
    //cvReleaseImage(&img);
}

void LaserFinder :: create(){
    prevX = 0;
    prevY = 0;
    prevH = 0;
    prevS = 0;
    prevV = 0;
    numOfImagesForFindAlike = 0;
    warp_matrix = cvCreateMat(3, 3, CV_32FC1);

    counter = 0;

    /*printf("topLeftX = %i\n", topLeftX);
    printf("topLeftY = %i\n", topLeftY);
    printf("topRightX = %i\n", topRightX);
    printf("topRightY = %i\n", topRightY);
    printf("bottomLeftX = %i\n", bottomLeftX);
    printf("bottomLeftY = %i\n", bottomLeftY);
    printf("bottomRightX = %i\n", bottomRightX);
    printf("bottomRightY = %i\n", bottomRightY);

    printf("Hmin = %i\n", Hmin);
    printf("Hmax = %i\n", Hmax);
    printf("Smin = %i\n", Smin);
    printf("Smax = %i\n", Smax);
    printf("Vmin = %i\n", Vmin);
    printf("Vmax = %i\n", Vmax);*/
}

bool LaserFinder :: getPoint(const IplImage* frame, int* x, int* y){
    if (frame == 0){
        return false;
    }
    //перспективное преобразование
    // Создаёи 8-битную, 3-канальную картинку
    IplImage* dst = cvCreateImage(cvSize(resultSizeX, resultSizeY), 8, 3);
    // заливаем картинку
    cvSet(dst,cvScalar(250,250,250));
    perspectiveTransform ( frame, dst );

    IplImage* trackImg = cvCreateImage( cvGetSize(dst), IPL_DEPTH_8U, 3 );
    // заливаем картинку цветом
    cvSet(trackImg,cvScalar(250,250,250));

    bool ifFound;
    ifFound = findLaser( dst, x, y );
    /*if ((numOfImagesForFindAlike == 0) || (numOfImagesForFindAlike == 5)){
        //вызываем функцию-обработчика
        ifFound = findLaser( dst, x, y );
        numOfImagesForFindAlike = 1;
    }else{
        ++(numOfImagesForFindAlike);
        ifFound = findAlike( dst, x, y );
        if (!ifFound){
            numOfImagesForFindAlike = 0;
            findLaser( dst, x, y );
        }
    }*/
    if (ifFound){
        prevX = *x;
        prevY = *y;            
        IplImage* prev_hsv = cvCreateImage( cvGetSize(dst), IPL_DEPTH_8U, 3 );
        cvCvtColor( dst, prev_hsv, CV_BGR2HSV ); 
        int prevStep      = prev_hsv->widthStep;
        int prevChannels  = prev_hsv->nChannels;
        uchar *prevData   = (uchar *)prev_hsv->imageData;
        prevH = prevData[prevX*prevStep + prevY*prevChannels + 0];
        prevS = prevData[prevX*prevStep + prevY*prevChannels + 1];
        prevV = prevData[prevX*prevStep + prevY*prevChannels + 2];
        cvReleaseImage(&prev_hsv);

        counter = 0;
        track.push_back(cvPoint(*x, *y));
        //рисуем целеуказатель
        drawTarget(trackImg, *x, *y, 10);
        drawTrack(trackImg);
    }else{
        ++counter;
        if (counter > 10){
            track.erase(track.begin(), track.end());
            //counter = 0;
        }
    }
    //показываем кадр
    //cvShowImage( "original", frame );
    cvShowImage( "Result", trackImg );
    //printf("%i X %i\n", *x, *y);

    cvReleaseImage(&dst);
    // !!!
    cvReleaseImage(&trackImg);

    return ifFound;
}

void LaserFinder :: setVideoStream (const char* videoStream, int cam_id){
    if (videoStream == NULL){
        //capture = cvCreateCameraCapture(CV_CAP_ANY);
        capture = cvCreateCameraCapture(cam_id);
        assert(capture);
    }else{
        //получаем информацию о видео-файле
        const char* filename = videoStream;
        capture = cvCreateFileCapture( filename );
    }
    printf("[i] press Esc for quit!\n\n");
}

void LaserFinder :: loadParams (const char* configFile){
    if (configFile != NULL){
        std :: ifstream in( configFile );
        in >> Hmin >> Hmax >> Smin >> Smax >> Vmin >> Vmax;
        in >> topLeftX >> topLeftY >> topRightX >> topRightY >> bottomLeftX >> bottomLeftY >> bottomRightX >> bottomRightY;
        in.close();
    }
    create();
}

IplImage* LaserFinder :: getImage() const{
        char c = cvWaitKey(33);
        if (c == 27) { // если нажата ESC - выходим
            return NULL;
        }

    return cvQueryFrame( capture );
}

void LaserFinder :: destroy(){
    cvReleaseMat(&warp_matrix);
    cvReleaseCapture( &capture );
    cvDestroyAllWindows();
}

LaserFinder::LaserFinder(const char* videoStream, const char* configFile, int cam_id, int res_x, int res_y){
    setVideoStream(videoStream, cam_id);
    loadParams(configFile);
    resultSizeX = res_x;
    resultSizeY = res_y;
    create();
    setCorners();
    // окно для отображения картинки
    //cvNamedWindow("original", CV_WINDOW_AUTOSIZE);
    cvNamedWindow( "Result", 0 );
}

LaserFinder::~LaserFinder(){
    destroy();
}

int LaserFinder::getEmptyCounter() const {
    return counter;
}
